/****************************************************
 ****************************************************
 ***                                              ***
 ***   Project:  YAVRS                            ***
 ***   (Yet Another Virtual Reality System)       ***
 ***   ________________________________________   ***
 ***                                              ***
 ***   File:     main_test.cpp                    ***
 ***   Authors:  Steve Conner & Thomas Marrinan   ***
 ***   Date:     09/28/2011                       ***
 ***                                              ***
 ****************************************************
 ****************************************************/

#ifdef __APPLE__
#include <OpenAL/al.h>
#include <OpenAL/alc.h>
#endif
#ifndef __APPLE__
#include <AL/alut.h>
#endif

#include <string.h>
#include <iostream>
#include "textureGen.h"
#include "yavrsCamera.h"
#include "yavrsOmega.h"
#include "yavrsWavefrontObj.h"

#define WIDTH  800
#define HEIGHT 600

#define Y_OFFSET 1.52
#define Z_OFFSET -1.7

#define RAD_TO_DEG 57.2957795

#define MOCAP_HEAD 0
#define MOCAP_WIIMOTE 2

#define WII_JOY_THRESHOLD 30

using namespace std;

ALCdevice *device;
ALCcontext *context;
ALfloat ListenerPos[] = {0.0, 1.8,  0.0};
ALfloat ListenerVel[] = {0.0, 0.0,  0.0};
ALfloat ListenerOri[] = {0.0, 0.0, -1.0, 0.0, 1.0, 0.0};

yavrsCamera *camera;
yavrsEventListener *eventListener;
yavrsEvent event;

// Models
yavrsWavefrontObj *flintstones;
yavrsWavefrontObj *jetsons;
yavrsWavefrontObj *simpsons;
yavrsWavefrontObj *planet_express;
yavrsWavefrontObj * selectedOBJ = NULL;

static GLuint texName[6];

double wii_0[3];
double wii_1[3];
double wii_col[3];

void drawScene() {
	// light position transforms along with model
	float light_position0[]  =  { 0.509, 0.601,  0.616, 0.00};
	float light_position1[]  =  {-0.509, 0.601, -0.616, 0.00};
	glLightfv(GL_LIGHT0, GL_POSITION, light_position0);
    glLightfv(GL_LIGHT1, GL_POSITION, light_position1);
	
	int i, j;
	GLUquadricObj *quadObj = gluNewQuadric();
    
    float white_color[] = {1.0, 1.0, 1.0, 1.0};
    float gray_color[] = {0.7, 0.7, 0.7, 1.0};
    float black_color[] = {0.1, 0.1, 0.1, 1.0};
    float blue_color[] = {0.1, 0.2, 0.5, 1.0};
	float red_color[] = {0.8, 0.2, 0.1, 1.0};
	float yellow_color[] = {0.8, 0.7, 0.1, 1.0};
    float shininess[] = {32.0};
	
	/* draw scene */
    
    // Model Stands
    glMaterialfv(GL_FRONT, GL_AMBIENT, blue_color);
	glMaterialfv(GL_FRONT, GL_DIFFUSE, blue_color);
	glMaterialfv(GL_FRONT, GL_SPECULAR, black_color);
	glMaterialfv(GL_FRONT, GL_SHININESS, shininess);
    glPushMatrix();
	glBegin(GL_QUADS);
	glNormal3f(0.0, 0.0, 1.0);
	glVertex3f(-5.1, 0.0, -1.4);
	glVertex3f(-4.4, 0.0, -1.4);
	glVertex3f(-4.4, 1.0, -1.4);
	glVertex3f(-5.1, 1.0, -1.4);
	glNormal3f(1.0, 0.0, 0.0);
	glVertex3f(-4.4, 0.0, -1.4);
	glVertex3f(-4.4, 0.0, -2.1);
	glVertex3f(-4.4, 1.0, -2.1);
	glVertex3f(-4.4, 1.0, -1.4);
	glNormal3f(0.0, 0.0, -1.0);
	glVertex3f(-4.4, 0.0, -2.1);
	glVertex3f(-5.1, 0.0, -2.1);
	glVertex3f(-5.1, 1.0, -2.1);
	glVertex3f(-4.4, 1.0, -2.1);
	glNormal3f(0.0, 1.0, 0.0);
	glVertex3f(-5.1, 1.0, -1.4);
	glVertex3f(-4.4, 1.0, -1.4);
	glVertex3f(-4.4, 1.0, -2.1);
	glVertex3f(-5.1, 1.0, -2.1);
	glEnd();
    glPopMatrix();
    glPushMatrix();
	glBegin(GL_QUADS);
	glNormal3f(0.0, 0.0, 1.0);
	glVertex3f(-5.1, 0.0, -8.4);
	glVertex3f(-4.4, 0.0, -8.4);
	glVertex3f(-4.4, 1.0, -8.4);
	glVertex3f(-5.1, 1.0, -8.4);
	glNormal3f(1.0, 0.0, 0.0);
	glVertex3f(-4.4, 0.0, -8.4);
	glVertex3f(-4.4, 0.0, -9.1);
	glVertex3f(-4.4, 1.0, -9.1);
	glVertex3f(-4.4, 1.0, -8.4);
	glNormal3f(0.0, 0.0, -1.0);
	glVertex3f(-4.4, 0.0, -9.1);
	glVertex3f(-5.1, 0.0, -9.1);
	glVertex3f(-5.1, 1.0, -9.1);
	glVertex3f(-4.4, 1.0, -9.1);
	glNormal3f(0.0, 1.0, 0.0);
	glVertex3f(-5.1, 1.0, -8.4);
	glVertex3f(-4.4, 1.0, -8.4);
	glVertex3f(-4.4, 1.0, -9.1);
	glVertex3f(-5.1, 1.0, -9.1);
	glEnd();
    glPopMatrix();
    glPushMatrix();
	glBegin(GL_QUADS);
	glNormal3f(0.0, 0.0, 1.0);
	glVertex3f(4.4, 0.0, -1.4);
	glVertex3f(5.1, 0.0, -1.4);
	glVertex3f(5.1, 1.0, -1.4);
	glVertex3f(4.4, 1.0, -1.4);
	glNormal3f(-1.0, 0.0, 0.0);
	glVertex3f(4.4, 0.0, -2.1);
	glVertex3f(4.4, 0.0, -1.4);
	glVertex3f(4.4, 1.0, -1.4);
	glVertex3f(4.4, 1.0, -2.1);
	glNormal3f(0.0, 0.0, -1.0);
	glVertex3f(5.1, 0.0, -2.1);
	glVertex3f(4.4, 0.0, -2.1);
	glVertex3f(4.4, 1.0, -2.1);
	glVertex3f(5.1, 1.0, -2.1);
	glNormal3f(0.0, 1.0, 0.0);
	glVertex3f(4.4, 1.0, -1.4);
	glVertex3f(5.1, 1.0, -1.4);
	glVertex3f(5.1, 1.0, -2.1);
	glVertex3f(4.4, 1.0, -2.1);
	glEnd();
    glPopMatrix();
    glPushMatrix();
	glBegin(GL_QUADS);
	glNormal3f(0.0, 0.0, 1.0);
	glVertex3f(4.4, 0.0, -8.4);
	glVertex3f(5.1, 0.0, -8.4);
	glVertex3f(5.1, 1.0, -8.4);
	glVertex3f(4.4, 1.0, -8.4);
	glNormal3f(-1.0, 0.0, 0.0);
	glVertex3f(4.4, 0.0, -9.1);
	glVertex3f(4.4, 0.0, -8.4);
	glVertex3f(4.4, 1.0, -8.4);
	glVertex3f(4.4, 1.0, -9.1);
	glNormal3f(0.0, 0.0, -1.0);
	glVertex3f(5.1, 0.0, -9.1);
	glVertex3f(4.4, 0.0, -9.1);
	glVertex3f(4.4, 1.0, -9.1);
	glVertex3f(5.1, 1.0, -9.1);
	glNormal3f(0.0, 1.0, 0.0);
	glVertex3f(4.4, 1.0, -8.4);
	glVertex3f(5.1, 1.0, -8.4);
	glVertex3f(5.1, 1.0, -9.1);
	glVertex3f(4.4, 1.0, -9.1);
	glEnd();
    glPopMatrix();
	
	// Cone
	glPushMatrix();
	glBegin(GL_TRIANGLES);
	for(i=0; i<360; i+=30){
		if(i%60 == 0){
			glMaterialfv(GL_FRONT, GL_AMBIENT, red_color);
			glMaterialfv(GL_FRONT, GL_DIFFUSE, red_color);
			glMaterialfv(GL_FRONT, GL_SPECULAR, gray_color);
			glMaterialfv(GL_FRONT, GL_SHININESS, shininess);
		}
		else{
			glMaterialfv(GL_FRONT, GL_AMBIENT, yellow_color);
			glMaterialfv(GL_FRONT, GL_DIFFUSE, yellow_color);
			glMaterialfv(GL_FRONT, GL_SPECULAR, gray_color);
			glMaterialfv(GL_FRONT, GL_SHININESS, shininess);
		}
		double cone_vert1[3], cone_vert2[3], cone_vert3[3]; 
		cone_vert1[0] = 0.5*cos(i*PI/180.0) + 0.0;
		cone_vert1[1] = 0.5*sin(i*PI/180.0) + 1.8;
		cone_vert1[2] = -15.0;
		cone_vert2[0] = 0.5*cos((i+30)*PI/180.0) + 0.0;
		cone_vert2[1] = 0.5*sin((i+30)*PI/180.0) + 1.8;
		cone_vert2[2] = -15.0;
		cone_vert3[0] = 0.0;
		cone_vert3[1] = 1.8;
		cone_vert3[2] = -13.0;
		double v1X = cone_vert2[0] - cone_vert1[0];
		double v1Y = cone_vert2[1] - cone_vert1[1];
		double v1Z = cone_vert2[2] - cone_vert1[2];
		double v2X = cone_vert3[0] - cone_vert1[0];
		double v2Y = cone_vert3[1] - cone_vert1[1];
		double v2Z = cone_vert3[2] - cone_vert1[2];
		double normX = v1Y*v2Z - v1Z*v2Y;
		double normY = v1Z*v2X - v1X*v2Z;
		double normZ = v1X*v2Y - v1Y*v2X;
		double nMag = sqrt(normX*normX + normY*normY + normZ*normZ);
		normX /= nMag;
		normY /= nMag;
		normZ /= nMag;
		glNormal3f(normX, normY, normZ);
		glVertex3f(cone_vert1[0], cone_vert1[1], cone_vert1[2]);
		glVertex3f(cone_vert2[0], cone_vert2[1], cone_vert2[2]);
		glVertex3f(cone_vert3[0], cone_vert3[1], cone_vert3[2]);
	}
	glEnd();
	glPopMatrix();
	
	// Cylinder in middle of room
	glMaterialfv(GL_FRONT, GL_AMBIENT, black_color);
	glMaterialfv(GL_FRONT, GL_DIFFUSE, black_color);
	glMaterialfv(GL_FRONT, GL_SPECULAR, gray_color);
	glMaterialfv(GL_FRONT, GL_SHININESS, shininess);
	glPushMatrix();
	glTranslatef(0.0, 1.2, -5.0);
	glRotatef(90.0, 1.0, 0.0, 0.0);
	gluCylinder(quadObj, 0.3, 0.3, 1.2, 12, 1);
	glPopMatrix();
	
	
    
    // Wii-mote pointer
    glDisable(GL_LIGHTING);
    glPushMatrix();
	glLineWidth(3);
	glBegin(GL_LINES);
	glColor3f(wii_col[0], wii_col[1], wii_col[2]);
	glVertex3f(wii_1[0], wii_1[1], wii_1[2]);
	glVertex3f(wii_0[0], wii_0[1], wii_0[2]);
	glEnd();
	glLineWidth(1);
    glPopMatrix();
    glEnable(GL_LIGHTING);
	
    
    // Textures
    glEnable(GL_TEXTURE_2D);
    glMaterialfv(GL_FRONT, GL_AMBIENT, white_color);
	glMaterialfv(GL_FRONT, GL_DIFFUSE, white_color);
	glMaterialfv(GL_FRONT, GL_SPECULAR, white_color);
	glMaterialfv(GL_FRONT, GL_SHININESS, shininess);
	
	// Posters
    glPushMatrix();
	glBindTexture(GL_TEXTURE_2D, texName[2]);
	glBegin(GL_QUADS);
	glNormal3f(1.0, 0.0, 0.0);
	glTexCoord2f(0.0, 0.0); glVertex3f(-4.99, 1.6,-1.375);
	glTexCoord2f(1.0, 0.0); glVertex3f(-4.99, 1.6,-2.125);
	glTexCoord2f(1.0, 1.0); glVertex3f(-4.99, 2.6,-2.125);
	glTexCoord2f(0.0, 1.0); glVertex3f(-4.99, 2.6,-1.375);
	glEnd();
	glPopMatrix();
	glPushMatrix();
	glBindTexture(GL_TEXTURE_2D, texName[3]);
	glBegin(GL_QUADS);
	glNormal3f(1.0, 0.0, 0.0);
	glTexCoord2f(0.0, 0.0); glVertex3f(-4.99, 1.6,-8.355);
	glTexCoord2f(1.0, 0.0); glVertex3f(-4.99, 1.6,-9.145);
	glTexCoord2f(1.0, 1.0); glVertex3f(-4.99, 2.6,-9.145);
	glTexCoord2f(0.0, 1.0); glVertex3f(-4.99, 2.6,-8.355);
	glEnd();
	glPopMatrix();
	glPushMatrix();
	glBindTexture(GL_TEXTURE_2D, texName[4]);
	glBegin(GL_QUADS);
	glNormal3f(-1.0, 0.0, 0.0);
	glTexCoord2f(0.0, 0.0); glVertex3f(4.99, 1.6,-9.1);
	glTexCoord2f(1.0, 0.0); glVertex3f(4.99, 1.6,-8.4);
	glTexCoord2f(1.0, 1.0); glVertex3f(4.99, 2.6,-8.4);
	glTexCoord2f(0.0, 1.0); glVertex3f(4.99, 2.6,-9.1);
	glEnd();
	glPopMatrix();
	glPushMatrix();
	glBindTexture(GL_TEXTURE_2D, texName[5]);
	glBegin(GL_QUADS);
	glNormal3f(-1.0, 0.0, 0.0);
	glTexCoord2f(0.0, 0.0); glVertex3f(4.99, 1.6,-2.075);
	glTexCoord2f(1.0, 0.0); glVertex3f(4.99, 1.6,-1.425);
	glTexCoord2f(1.0, 1.0); glVertex3f(4.99, 2.6,-1.425);
	glTexCoord2f(0.0, 1.0); glVertex3f(4.99, 2.6,-2.075);
	glEnd();
	glPopMatrix();
    
	// Textured Floor
    glPushMatrix();
	glBindTexture(GL_TEXTURE_2D, texName[0]);
	glBegin(GL_QUADS);
	glNormal3f(0.0, 1.0, 0.0);
	glTexCoord2f( 0.0, 20.0); glVertex3f(-5.0, 0.0,  5.0);
	glTexCoord2f(10.0, 20.0); glVertex3f( 5.0, 0.0,  5.0);
	glTexCoord2f(10.0,  0.0); glVertex3f( 5.0, 0.0,-15.0);
	glTexCoord2f( 0.0,  0.0); glVertex3f(-5.0, 0.0,-15.0);
	glEnd();
    glPopMatrix();
    
    // Textured Walls
    glPushMatrix();
	glBindTexture(GL_TEXTURE_2D, texName[1]);
	glBegin(GL_QUADS);
	glNormal3f(0.0, 0.0, 1.0);
	glTexCoord2f( 0.0, 3.0); glVertex3f(-5.0, 3.0,-15.0);
	glTexCoord2f(10.0, 3.0); glVertex3f( 5.0, 3.0,-15.0);
	glTexCoord2f(10.0, 0.0); glVertex3f( 5.0, 0.0,-15.0);
	glTexCoord2f( 0.0, 0.0); glVertex3f(-5.0, 0.0,-15.0);
	glNormal3f(0.0, 0.0,-1.0);
	glTexCoord2f( 0.0, 3.0); glVertex3f( 5.0, 3.0,  5.0);
	glTexCoord2f(10.0, 3.0); glVertex3f(-5.0, 3.0,  5.0);
	glTexCoord2f(10.0, 0.0); glVertex3f(-5.0, 0.0,  5.0);
	glTexCoord2f( 0.0, 0.0); glVertex3f( 5.0, 0.0,  5.0);
	glNormal3f(1.0, 0.0, 0.0);
	glTexCoord2f( 0.0, 3.0); glVertex3f(-5.0, 3.0,  5.0);
	glTexCoord2f(20.0, 3.0); glVertex3f(-5.0, 3.0,-15.0);
	glTexCoord2f(20.0, 0.0); glVertex3f(-5.0, 0.0,-15.0);
	glTexCoord2f( 0.0, 0.0); glVertex3f(-5.0, 0.0,  5.0);
	glNormal3f(-1.0, 0.0, 0.0);
	glTexCoord2f( 0.0, 3.0); glVertex3f( 5.0, 3.0,-15.0);
	glTexCoord2f(20.0, 3.0); glVertex3f( 5.0, 3.0,  5.0);
	glTexCoord2f(20.0, 0.0); glVertex3f( 5.0, 0.0,  5.0);
	glTexCoord2f( 0.0, 0.0); glVertex3f( 5.0, 0.0,-15.0);
	glEnd();
    glPopMatrix();
    glDisable(GL_TEXTURE_2D);
    
    // Ceiling
    glMaterialfv(GL_FRONT, GL_AMBIENT, gray_color);
	glMaterialfv(GL_FRONT, GL_DIFFUSE, gray_color);
	glMaterialfv(GL_FRONT, GL_SPECULAR, white_color);
	glMaterialfv(GL_FRONT, GL_SHININESS, shininess);
    glPushMatrix();
	glBegin(GL_QUADS);
	glNormal3f(0.0, 0.0, 1.0);
	glVertex3f(-5.0, 3.0,  5.0);
	glVertex3f( 5.0, 3.0,  5.0);
	glVertex3f( 5.0, 3.0,-15.0);
	glVertex3f(-5.0, 3.0,-15.0);
	glEnd();
    glPopMatrix();
    // Ceiling Pipes
    glMaterialfv(GL_FRONT, GL_AMBIENT, black_color);
	glMaterialfv(GL_FRONT, GL_DIFFUSE, black_color);
	glMaterialfv(GL_FRONT, GL_SPECULAR, gray_color);
	glMaterialfv(GL_FRONT, GL_SHININESS, shininess);
    for(i=-4; i<5; i++){
        glPushMatrix();
		glTranslatef(i, 2.95, -15.0);
		gluCylinder(quadObj, 0.03, 0.03, 20.0, 12, 1);
        glPopMatrix();
    }
    for(i=-13; i<5; i+=2){
        glPushMatrix();
		glTranslatef(-5.0, 2.95, i);
		glRotatef(90.0, 0.0, 1.0, 0.0);
		gluCylinder(quadObj, 0.03, 0.03, 10.0, 12, 1);
        glPopMatrix();
    }
    
    // OBJ Models
	flintstones->draw();
    jetsons->draw();
    simpsons->draw();
	planet_express->draw();
}

void handleEvents()
{
	switch(event.type) {
		case YAVRS_MOCAP_EVENT:
			if(event.sourceID == MOCAP_HEAD) {
				camera->setCOP(event.data.mocap.x, event.data.mocap.y - Y_OFFSET, event.data.mocap.z - Z_OFFSET);

				double tmp[3];
				camera->getCameraPosition(tmp);
				ListenerPos[0] = tmp[0];
				ListenerPos[1] = tmp[1];
				ListenerPos[2] = tmp[2];

				camera->getWindowDirection(tmp);
				ListenerOri[0] = tmp[0];
				ListenerOri[1] = tmp[1];
				ListenerOri[2] = tmp[2];
				camera->getWindowUp(tmp);
				ListenerOri[3] = tmp[0];
				ListenerOri[4] = tmp[1];
				ListenerOri[5] = tmp[2];

				alListenerfv(AL_POSITION,    ListenerPos);
				alListenerfv(AL_ORIENTATION, ListenerOri);

			//} else if(event.sourceID == 1) {
			} else if(event.sourceID == MOCAP_WIIMOTE) {
				float scale = sqrt(event.data.mocap.q.x * event.data.mocap.q.x + event.data.mocap.q.y * event.data.mocap.q.y + event.data.mocap.q.z * event.data.mocap.q.z);
				double cow[3];
				double wDir[3];
				double wUp[3];
				double wRight[3];
				camera->getWindowCenter(cow);
				camera->getWindowDirection(wDir);
				camera->getWindowUp(wUp);

				double modelPosCam[3];
				double modelPosWC[3];
				modelPosCam[0] = event.data.mocap.x;
				modelPosCam[1] = event.data.mocap.y-Y_OFFSET;
				modelPosCam[2] = event.data.mocap.z-Z_OFFSET-1.0;
				camera->vrc2world(modelPosCam, modelPosWC);
				
				// wRight vector = wDir X wUp
				wRight[0] = wDir[1]*wUp[2] - wDir[2]*wUp[1];
				wRight[1] = wDir[2]*wUp[0] - wDir[0]*wUp[2];
				wRight[2] = wDir[0]*wUp[1] - wDir[1]*wUp[0];

				double modelRX =  wRight[0]*(event.data.mocap.q.x) + wUp[0]*(event.data.mocap.q.y) + wDir[0]*(event.data.mocap.q.z);
				double modelRY =  wRight[1]*(event.data.mocap.q.x) + wUp[1]*(event.data.mocap.q.y) + wDir[1]*(event.data.mocap.q.z);
				double modelRZ = -wRight[2]*(event.data.mocap.q.x) - wUp[2]*(event.data.mocap.q.y) - wDir[2]*(event.data.mocap.q.z);

				double modelX = modelPosWC[0];
				double modelY = modelPosWC[1];
				double modelZ = modelPosWC[2];

				//planet_express->setPosition(modelX, modelY, modelZ);
				//planet_express->setRotation(acos(event.data.mocap.q.w) * 2.0f * RAD_TO_DEG, modelRX / scale, modelRY / scale, modelRZ / scale);
                
                // Wii-mote Ray
                double wiiCam[3], wiiWC[3];
                double wiiPointerCam[3], wiiPointerWC[3];
				wiiCam[0] = event.data.mocap.x;
				wiiCam[1] = event.data.mocap.y-Y_OFFSET;
				wiiCam[2] = event.data.mocap.z-Z_OFFSET;
                wiiPointerCam[0] = event.data.mocap.x;
				wiiPointerCam[1] = event.data.mocap.y-Y_OFFSET;
				wiiPointerCam[2] = event.data.mocap.z-Z_OFFSET-50.0;
				
                camera->vrc2world(wiiCam, wii_0);
                camera->vrc2world(wiiPointerCam, wiiPointerWC);
                
                double wiiPointerRX =  wRight[0]*(event.data.mocap.q.x) + wUp[0]*(event.data.mocap.q.y);// + wDir[0]*(event.data.mocap.q.z);
				double wiiPointerRY =  wRight[1]*(event.data.mocap.q.x) + wUp[1]*(event.data.mocap.q.y);// + wDir[1]*(event.data.mocap.q.z);
				double wiiPointerRZ =  wRight[2]*(event.data.mocap.q.x) + wUp[2]*(event.data.mocap.q.y);// + wDir[2]*(event.data.mocap.q.z);
                
                double x = wiiPointerWC[0];
                double y = wiiPointerWC[1];
                double z = wiiPointerWC[2];
                double a = wii_0[0];
                double b = wii_0[1];
                double c = wii_0[2];
                double u = wiiPointerRX / scale;
                double v = wiiPointerRY / scale;
                double w = wiiPointerRZ / scale;
                double T = acos(event.data.mocap.q.w) * 2.0f;
                
                wii_1[0] = (a*(v*v+w*w)-u*(b*v+c*w-u*x-v*y-w*z))*(1-cos(T))+x*cos(T)+(-c*v+b*w-w*y+v*z)*sin(T);
                wii_1[1] = (b*(u*u+w*w)-v*(a*u+c*w-u*x-v*y-w*z))*(1-cos(T))+y*cos(T)+( c*u-a*w+w*x-u*z)*sin(T);
                wii_1[2] = (c*(u*u+v*v)-w*(a*u+b*v-u*x-v*y-w*z))*(1-cos(T))+z*cos(T)+(-b*u+a*v-v*x+u*y)*sin(T);
                
                double flintIntersect = flintstones->rayCollision(wii_0, wii_1);
                double jetsIntersect  = jetsons->rayCollision(wii_0, wii_1);
                double simpIntersect  = simpsons->rayCollision(wii_0, wii_1);
                double futrmIntersect = planet_express->rayCollision(wii_0, wii_1);
                
                double closestOBJ = 9.9e12;
                if(flintIntersect > 0.0 && flintIntersect < closestOBJ) {
                    closestOBJ = flintIntersect;
                    selectedOBJ = flintstones;
                }
                if(jetsIntersect > 0.0 && jetsIntersect < closestOBJ) {
                    closestOBJ = jetsIntersect;
                    selectedOBJ = jetsons;
                }
                if(simpIntersect  > 0.0 && simpIntersect  < closestOBJ) {
                    closestOBJ = simpIntersect;
                    selectedOBJ = simpsons;
                }
                if(futrmIntersect > 0.0 && futrmIntersect < closestOBJ) {
                    closestOBJ = futrmIntersect;
                    selectedOBJ = planet_express;
                }
                if(closestOBJ >= 9.9e11) {
                    closestOBJ = -1.0;
                    selectedOBJ = NULL;
                }

                if(closestOBJ > 0.0){
                    wii_col[0] = 0.0;
                    wii_col[1] = 0.1;
                    wii_col[2] = 0.9;
                }
                else{
                    wii_col[0] = 0.9;
                    wii_col[1] = 0.1;
                    wii_col[2] = 0.0;
                }
                
			} else {
				//printf("Unknown mocap device id\n");
			}
			break;
		case YAVRS_WIIMOTE_EVENT:
			double tmp[3];
			camera->getCameraPosition(tmp);
			ListenerPos[0] = tmp[0];
			ListenerPos[1] = tmp[1];
			ListenerPos[2] = tmp[2];
			alListenerfv(AL_POSITION,    ListenerPos);
			alListenerfv(AL_ORIENTATION, ListenerOri);

			if(event.data.wiimote.up) {
				camera->translateCamera(0.0, 0.0, 0.02);
			}
			if(event.data.wiimote.down) {
				camera->translateCamera(0.0, 0.0, -0.02);
			}
			if(event.data.wiimote.left) {
				camera->translateCamera(-0.02, 0.0, 0.0);
			}
			if(event.data.wiimote.right) {
				camera->translateCamera(0.02, 0.0, 0.0);
			}
			if(event.data.wiimote.plus) {
				camera->translateCamera(0.0, 0.01, 0.0);
			}
			if(event.data.wiimote.minus) {
				camera->translateCamera(0.0, -0.01, 0.0);
			}
			if(event.data.wiimote.a) {
				if(selectedOBJ) {
					selectedOBJ->playWaveFile();
				}
			}
			if(event.data.wiimote.b) {
				
			}
			break;
		case YAVRS_NUNCHUK_EVENT:
			camera->getWindowDirection(tmp);
			ListenerOri[0] = tmp[0];
			ListenerOri[1] = tmp[1];
			ListenerOri[2] = tmp[2];
			camera->getWindowUp(tmp);
			ListenerOri[3] = tmp[0];
			ListenerOri[4] = tmp[1];
			ListenerOri[5] = tmp[2];

			alListenerfv(AL_POSITION,    ListenerPos);
			alListenerfv(AL_ORIENTATION, ListenerOri);

			if(event.data.nunchuk.analogX < -WII_JOY_THRESHOLD) {
				//printf("%d,", event.data.nunchuk.analogX);
				camera->lookLeft(-event.data.nunchuk.analogX / 500.0);
			} else if(event.data.nunchuk.analogX > WII_JOY_THRESHOLD) {
				//printf("%d,", event.data.nunchuk.analogX);
				camera->lookRight(event.data.nunchuk.analogX / 500.0);
			}

			if(event.data.nunchuk.analogY < -WII_JOY_THRESHOLD) {
				//printf("%d\n", event.data.nunchuk.analogY);
				camera->lookDown(-event.data.nunchuk.analogY / 500.0);
			} else if(event.data.nunchuk.analogY > WII_JOY_THRESHOLD) {
				//printf("%d\n", event.data.nunchuk.analogY);
				camera->lookUp(event.data.nunchuk.analogY / 500.0);
			}
			break;
		default:
			printf("Unknown event type %d\n", event.type);
			break;
	}
}

void display() {	
	/* clear the screen to the clear color */
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	if(eventListener != NULL){
		while(eventListener->getNextEvent(&event)) {
			handleEvents();
		}
	}
	
	/* draw everything */
	camera->renderScene();
	
	/* swap buffers */	
	glutSwapBuffers();
}

void keyboard(unsigned char key, int x, int y) {
	switch(key) {
		case 'p':
			camera->translateCOP(0.0, 0.0, 0.03);
			break;
		case 'P':
			camera->translateCOP(0.0, 0.0, -0.03);
			break;
		case 'd':
			camera->translateCamera(0.1, 0.0, 0.0);
			break;
		case 'a':
			camera->translateCamera(-0.1, 0.0, 0.0);
			break;
		case 'w':
			camera->translateCamera(0.0, 0.1, 0.0);
			break;
		case 's':
			camera->translateCamera(0.0, -0.1, 0.0);
			break;
		case 'q':
			camera->translateCamera(0.0, 0.0, 0.1);
			break;
		case 'Q':
			camera->translateCamera(0.0, 0.0, -0.1);
			break;
		case 'x':
			camera->lookUp(3.0);
			break;
		case 'X':
			camera->lookDown(3.0);
			break;
		case 'y':
			camera->lookLeft(3.0);
			break;
		case 'Y':
			camera->lookRight(3.0);
			break;
		case 'z':
			camera->lookCounterClockWise(3.0);
			break;
		case 'Z':
			camera->lookClockWise(3.0);
			break;
		case 'F':
			glutPositionWindow(0, 0);
			glutReshapeWindow(2732, 1536);
			break;
		case 'S':
			jetsons->playWaveFile();
			break;
	}
	
	camera->updatePerspective();
	glutPostRedisplay();
}

void specialKey(int key, int x, int y) {
	switch (key) {
		case GLUT_KEY_RIGHT:
			camera->translateCOP(0.03, 0.0, 0.0);
			break;
		case GLUT_KEY_LEFT:
			camera->translateCOP(-0.03, 0.0, 0.0);
			break;
		case GLUT_KEY_UP:
			camera->translateCOP(0.0, 0.03, 0.0);
			break;
		case GLUT_KEY_DOWN:
			camera->translateCOP(0.0, -0.03, 0.0);
			break;
	}
	
	camera->updatePerspective();
	glutPostRedisplay();
}

void mouseButton(int button, int state, int x, int y) {
	if(button == GLUT_LEFT_BUTTON && state == GLUT_DOWN){
		if(selectedOBJ) {
			selectedOBJ->playWaveFile();
		}
	}
}

void mouseMove(int x, int y) {
	if(eventListener == NULL){
		double camTmp[3];
		double winRT[3], winLT[3], winRB[3], winLB[3];
		int size[2];
		camera->getCameraPosition(camTmp);
		camera->getWindowRT(winRT);
		camera->getWindowLT(winLT);
		camera->getWindowRB(winRB);
		camera->getWindowLB(winLB);
		camera->getScreenSize(size);
		
		wii_0[0] = camTmp[0] + 0.05;
		wii_0[1] = camTmp[1] - 0.10;
		wii_0[2] = camTmp[2] - 0.20;
		
		double tX = (double)x / (double)size[0];
		double tY = (double)y / (double)size[1];
		
		double tmpVecX[3], tmpVecY[3];
		tmpVecX[0] = (winRT[0]-winLT[0])*tX;
		tmpVecX[1] = (winRT[1]-winLT[1])*tX;
		tmpVecX[2] = (winRT[2]-winLT[2])*tX;
		tmpVecY[0] = (winLB[0]-winLT[0])*tY;
		tmpVecY[1] = (winLB[1]-winLT[1])*tY;
		tmpVecY[2] = (winLB[2]-winLT[2])*tY;
		
		double tmpWii[3];
		tmpWii[0] = winLT[0] + tmpVecX[0] + tmpVecY[0];
		tmpWii[1] = winLT[1] + tmpVecX[1] + tmpVecY[1];
		tmpWii[2] = winLT[2] + tmpVecX[2] + tmpVecY[2];
		
		double wiiVec[3];
		wiiVec[0] = tmpWii[0] - wii_0[0];
		wiiVec[1] = tmpWii[1] - wii_0[1];
		wiiVec[2] = tmpWii[2] - wii_0[2];
		wii_1[0] = wii_0[0] + wiiVec[0]*50.0;
		wii_1[1] = wii_0[1] + wiiVec[1]*50.0;
		wii_1[2] = wii_0[2] + wiiVec[2]*50.0;
		
		double flintIntersect = flintstones->rayCollision(wii_0, wii_1);
		double jetsIntersect  = jetsons->rayCollision(wii_0, wii_1);
		double simpIntersect  = simpsons->rayCollision(wii_0, wii_1);
		double futrmIntersect = planet_express->rayCollision(wii_0, wii_1);
		
		double closestOBJ = 9.9e12;
		if(flintIntersect > 0.0 && flintIntersect < closestOBJ) {
			closestOBJ = flintIntersect;
			selectedOBJ = flintstones;
		}
		if(jetsIntersect > 0.0 && jetsIntersect < closestOBJ) {
			closestOBJ = jetsIntersect;
			selectedOBJ = jetsons;
		}
		if(simpIntersect  > 0.0 && simpIntersect  < closestOBJ) {
			closestOBJ = simpIntersect;
			selectedOBJ = simpsons;
		}
		if(futrmIntersect > 0.0 && futrmIntersect < closestOBJ) {
			closestOBJ = futrmIntersect;
			selectedOBJ = planet_express;
		}
		if(closestOBJ >= 9.9e11) {
			closestOBJ = -1.0;
			selectedOBJ = NULL;
		}
		
		if(closestOBJ > 0.0){
			wii_col[0] = 0.0;
			wii_col[1] = 0.1;
			wii_col[2] = 0.9;
		}
		else{
			wii_col[0] = 0.9;
			wii_col[1] = 0.1;
			wii_col[2] = 0.0;
		}
	}
}

void reshapeFunc(int w, int h) {
	camera->resize(w, h);
}

void idleFunc() {
	glutPostRedisplay(); // redraw display function
}

void menu(int selection) {
	switch(selection) {
		case 1:
			camera->setStereoMode(NONE);
			break;
		case 2:
			camera->setStereoMode(LEFT_EYE);
			break;
		case 3:
			camera->setStereoMode(RIGHT_EYE);
			break;
		case 4:
			camera->setStereoMode(INTERLEAVED);
			break;
		case 5:
			camera->setStereoMode(SIDE_BY_SIDE);
			break;
		case 6:
			camera->setInteractionMode(FREE_ROTATE);
			break;
		case 7:
			camera->setInteractionMode(FIRST_PERSON);
			break;
	}
}

void init() {
	// Initialize OpenAL
		device = alcOpenDevice(NULL);
	if(!device){
		printf("No Sound Device\n");
		return;
	}
	context = alcCreateContext(device, NULL);
	alcMakeContextCurrent(context);
	if(!context){
		printf("No Sound Context\n");
		return;
	}
 
	// initialize VR Camera
	double cw[3], cDir[3], cUp[3], cop[3];
	cw[0]   =  0.0;  cw[1]   =  1.5;  cw[2]   =  0.0;
	cDir[0] =  0.0;  cDir[1] =  0.0;  cDir[2] = -1.0;
	cUp[0]  =  0.0;  cUp[1]  =  1.0;  cUp[2]  =  0.0;
	cop[0]  =  0.0;  cop[1]  =  0.0;  cop[2]  =  1.7;
	
	camera = new yavrsCamera(WIDTH, HEIGHT, "../config/evl_vr_system.cfg");
	camera->renderFunc(drawScene);
	camera->setView(cw, cDir, cUp, cop);
	camera->setStereoMode(NONE);
    camera->setInteractionMode(FIRST_PERSON);
	
    // lighting values
	GLfloat ambient_light[]   =  {0.20, 0.20, 0.20, 1.00};
	GLfloat white_light[]     =  {1.00, 1.00, 1.00, 1.00};
	
	// initialize light settings
	glLightfv(GL_LIGHT0, GL_DIFFUSE, white_light);
	glLightfv(GL_LIGHT0, GL_SPECULAR, white_light);
    glLightfv(GL_LIGHT1, GL_DIFFUSE, white_light);
	glLightfv(GL_LIGHT1, GL_SPECULAR, white_light);
	glLightModelfv(GL_LIGHT_MODEL_AMBIENT, ambient_light);
	
	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);
    glEnable(GL_LIGHT1);
    glEnable(GL_NORMALIZE);
	glEnable(GL_DEPTH_TEST);
	glEnable(GL_BLEND);
	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	
	glShadeModel(GL_SMOOTH);
	
	// initialize 3D perspective projection
	camera->updatePerspective();
	
	// set background color to pale blue
	glClearColor(0.5, 0.7, 0.9, 1.0);

	//eventListener = new yavrsEventListener("euclid.evl.uic.edu");
    eventListener = NULL;
	
    wii_0[0] = -9.9e12;
    wii_0[1] = -9.9e12;
    wii_0[2] = -9.9e12;
    wii_1[0] = -9.9e12;
    wii_1[1] = -9.9e12;
    wii_1[2] = -9.9e12;
    wii_col[0] = 0.9;
    wii_col[1] = 0.1;
    wii_col[2] = 0.0;
    
    flintstones = new yavrsWavefrontObj();
	flintstones->load("models/flintstones.obj");
	flintstones->loadWaveFile("audio/Flintstones.wav");
    flintstones->normalizeObject();
    double *flintBounds = flintstones->getBounds();
	flintstones->setPosition(-4.75, 1.0-(flintBounds[2]*0.5), -1.75);
	flintstones->setRotation(0.0, 0.0, 0.0, 0.0);
	flintstones->setScale(0.5, 0.5, 0.5);
    
    jetsons = new yavrsWavefrontObj();
	jetsons->load("models/jetsons.obj");
	jetsons->loadWaveFile("audio/Jetsons.wav");
    jetsons->normalizeObject();
    double *jetsBounds = jetsons->getBounds();
	jetsons->setPosition(-4.75, 1.0-(jetsBounds[2]*0.55), -8.75);
	jetsons->setRotation(-90.0, 0.0, 1.0, 0.0);
	jetsons->setScale(0.55, 0.55, 0.55);
    
    simpsons = new yavrsWavefrontObj();
    simpsons->load("models/simpsons.obj");
    simpsons->loadWaveFile("audio/Simpsons.wav");
    simpsons->normalizeObject();
    double *simpBounds = simpsons->getBounds();
	simpsons->setPosition(4.75, 1.0-(simpBounds[2]*0.55), -8.75);
	simpsons->setRotation(180.0, 0.0, 1.0, 0.0);
	simpsons->setScale(0.55, 0.55, 0.55);
    
    
    planet_express = new yavrsWavefrontObj();
	planet_express->load("models/planet_express.obj");
	planet_express->loadWaveFile("audio/Futurama.wav");
    planet_express->normalizeObject();
    double *futrmBounds = planet_express->getBounds();
	planet_express->setPosition(4.75, 1.0-(futrmBounds[2]*0.75), -1.75);
	planet_express->setRotation(-90.0, 0.0, 1.0, 0.0);
	planet_express->setScale(0.75, 0.75, 0.75);
	
    textureGen* floorTex = new textureGen(512, 512);
    floorTex->readBMP("textures/tile floor.bmp");
    
    textureGen* wallTex = new textureGen(512, 512);
    wallTex->readBMP("textures/wall.bmp");
	
	textureGen* flintPoster = new textureGen(512, 1024);
	flintPoster->readBMP("textures/flintstones poster.bmp");
	
	textureGen* jetsPoster = new textureGen(512, 1024);
	jetsPoster->readBMP("textures/jetsons poster.bmp");
	
	textureGen* simpPoster = new textureGen(512, 1024);
	simpPoster->readBMP("textures/simpsons poster.bmp");
	
	textureGen* futrmPoster = new textureGen(512, 1024);
	futrmPoster->readBMP("textures/futurama poster.bmp");
    
    glGenTextures(6, texName);
	
	glBindTexture(GL_TEXTURE_2D, texName[0]);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, floorTex->getWidth(), floorTex->getHeight(), 0, GL_RGB, GL_UNSIGNED_BYTE, floorTex->getTexture());
    
    glBindTexture(GL_TEXTURE_2D, texName[1]);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, wallTex->getWidth(), wallTex->getHeight(), 0, GL_RGB, GL_UNSIGNED_BYTE, wallTex->getTexture());
	
	glBindTexture(GL_TEXTURE_2D, texName[2]);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, flintPoster->getWidth(), flintPoster->getHeight(), 0, GL_RGB, GL_UNSIGNED_BYTE, flintPoster->getTexture());
	
	glBindTexture(GL_TEXTURE_2D, texName[3]);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, jetsPoster->getWidth(), jetsPoster->getHeight(), 0, GL_RGB, GL_UNSIGNED_BYTE, jetsPoster->getTexture());
	
	glBindTexture(GL_TEXTURE_2D, texName[4]);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, simpPoster->getWidth(), simpPoster->getHeight(), 0, GL_RGB, GL_UNSIGNED_BYTE, simpPoster->getTexture());
	
	glBindTexture(GL_TEXTURE_2D, texName[5]);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, futrmPoster->getWidth(), futrmPoster->getHeight(), 0, GL_RGB, GL_UNSIGNED_BYTE, futrmPoster->getTexture());
    
    delete floorTex;
    delete wallTex;
	delete flintPoster;
	delete jetsPoster;
	delete simpPoster;
	delete futrmPoster;
}

int main(int argc, char** argv) {
	/* Deal with any GLUT command Line options */
	glutInit(&argc, argv);
	
	/* Create an output window */
	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH | GLUT_STENCIL);
	
	// Set up the physical window size
	glutInitWindowSize(WIDTH, HEIGHT);
	// Set the name of the window and try to create it
	glutCreateWindow("YAVRS Demo App");
	
	/* Set up the logical graphics space */
	init();
	
	/* Receive keyboard inputs */
	glutKeyboardFunc(keyboard);
	/* Receive keyboard inputs */
	glutSpecialFunc(specialKey);
	/* Receive mouse button input*/
	glutMouseFunc(mouseButton);
	glutPassiveMotionFunc(mouseMove);
	/* Assign the reshape function */
	glutReshapeFunc(reshapeFunc);
	/* Assign the display function */
	glutDisplayFunc(display);
	/* Assign the idle function */
	glutIdleFunc(idleFunc);
	
	/* menu attached to right mouse button */
	int mainMenu, subMenu1, subMenu2;
	subMenu1 = glutCreateMenu(menu);
	glutAddMenuEntry("None", 1);
	glutAddMenuEntry("Left Eye", 2);
	glutAddMenuEntry("Right Eye", 3);
	glutAddMenuEntry("Interleaved Stereo", 4);
	glutAddMenuEntry("Side-by-Side Stereo", 5);
	subMenu2 = glutCreateMenu(menu);
	glutAddMenuEntry("Free Rotation", 6);
	glutAddMenuEntry("First Person", 7);
	mainMenu = glutCreateMenu(menu);
	glutAddSubMenu("2D/3D Stereo Mode", subMenu1);
	glutAddSubMenu("Interaction Mode", subMenu2);
	glutAttachMenu(GLUT_RIGHT_BUTTON);
	
	glutMainLoop();
	return (0);
}
